//
// Created by nvidia on 2021/4/28.
//
#include "Serial.h"
#include <string.h>
#include <unistd.h>
#include <fcntl.h>
#include <termios.h>
#include "systime.h"
#include "Setter.h"
#include "log.h"

extern cv::Mat show_pic;
extern rm::Setter rm_setter;

Txpackage Txpackage::operator() (const cv::Point3f &pyz, const int &id,const int &armor_size_tag,const int &shoot_flag){
    cv::Point3f tmp_pyz=pyz;
    static short int p_tmp, y_tmp;
    static unsigned short int z_tmp;
    short int * t_ptr = (short int *)(t_buf+1);              //short int 2个字节
    //t_buf+1等价于使指针开头指向第二个元素t_buf[1],再把从这个位置开始的指针设置为sint * 类型，即占据2个字节（即t_ptr实际为t_buf[1]与t_buf[2])
    //t_buf+1指定指针开头，(sint *)指定数据大小
#if SHOOTER_MODE == DOWN_SHOOTER
//    float pitch_mul=1+(pow(abs(pyz.x),2)/200.0);
//    float yaw_mul=1+(pow(abs(pyz.y),2)/400.0);
    float pitch_mul=1;
    float yaw_mul=1;
#else
      float pitch_mul=1;
      float yaw_mul=1;
//      if(tmp_pyz.y>4) tmp_pyz.y=4;
//    if(tmp_pyz.x>8) tmp_pyz.x=8;
#endif
    p_tmp = static_cast<short>(pitch_mul*tmp_pyz.x*100)*rm_setter.serial_pitch_angle_plus_coefficient;//static_cast进行无关指针的强制类型转换 *100可以把小数点后两位传给电控
    y_tmp = static_cast<short>(yaw_mul*tmp_pyz.y*100)*rm_setter.serial_yaw_angle_plus_coefficient;
    z_tmp = static_cast<unsigned short>(tmp_pyz.z);
    unsigned  char id_tmp=static_cast<unsigned  char>(id);
//    if(shoot_flag==0||-1) cout<<"shoot_flag:"<<shoot_flag<<endl;
        switch (shoot_flag){
            case 1:t_buf[0]=0x1F;break;
            case 0:t_buf[0]=0xFF;break;
            case -1:t_buf[0]=0xFF;
                std::cerr<<"[ERROR] SHOOT_FLAG in sended serial uninited"<<std::endl;
                break;
        }
//    if(shoot_flag==1) t_buf[0]=0x1F;
    t_ptr[0]=y_tmp;//char[1],char[2]
    t_ptr[1]=p_tmp;//char[3],char[4]
    t_buf[5]=static_cast<unsigned char>(id);//armor_ID
    t_buf[6]=static_cast<unsigned char>(armor_size_tag);//NO:0;small:1;big:2;
    t_ptr[3]=z_tmp;//char[7],char[8]
    t_buf[9]= (unsigned char)(int(t_buf[0]+t_buf[3]+t_buf[5]) % 0XFF);
//    std::cout<<p_tmp<<std::endl;
#ifdef SHOW_SERIAL_INFO
    LOGM("[SEND SERIAL]");
    LOGM("[SEND SERIAL]:flag;%d,  yaw:%d, pitch:%d, id:%d, size:%d, dis: %d, end:%d",int(t_buf[0]),int(y_tmp),int(p_tmp),int(t_buf[5]),int(t_buf[6]),int(z_tmp),int(t_buf[9]));
#endif
    sprintf(send_message1,"[SEND]:flag:%d, yaw:%d, pitch:%d",int(t_buf[0]),int(y_tmp),int(p_tmp));
    sprintf(send_message2,"       id:%d, size:%d, dis:%d, end:%d",int(t_buf[5]),int(t_buf[6]),int(z_tmp),int(t_buf[9]));
};

bool sendTarget(Serial &serial,Txpackage txpackage) {
    return serial.WriteData(txpackage.t_buf, sizeof(txpackage.t_buf));
}

bool readTarget(Serial &serial){
//    unsigned  char r_buf[19];
    int data_length=23,tmp_length=2*data_length;
    unsigned  char tmp_buf[tmp_length];
    unsigned  char *r_buf;
    Rxpackage &rxpackage=serial.rxpackage;
//    if(!serial.ReadData(r_buf,19)) {
    tcflush(serial.fd, TCIFLUSH);
    if(!serial.ReadData(tmp_buf,tmp_length)) {
        rxpackage.success_flag=false;
        return false;
    }
    int i=0;
    while((unsigned short int)tmp_buf[i++]!=122&&(tmp_length-i)>=data_length) r_buf=tmp_buf+i;
//for(;(tmp_length-i)>data_length;i++) printf("%d:%d\n",i,(unsigned short int)tmp_buf[i]);
    if(i==tmp_length-data_length+1) {
        LOGE("Can't find correct serial head!");
        return false;
    }
    unsigned short int rcr = (unsigned short int)r_buf[1]+
                             (unsigned short int)r_buf[3]+
                             (unsigned short int)r_buf[5];
    rcr = rcr % 255;
//    unsigned int *ptr=( unsigned int *)(r_buf);
//    for(int i=0;i<data_length/8;i++){
//        printf("%d:%08x\n",i,ptr[i]);;
//    }

if((r_buf[0]==0x7a)&&(r_buf[data_length-1]==0x7b)){
    serial.last_t=serial.t;
    systime t;
    serial.t= getsystime(t);
        rxpackage.bullet_spd = int(r_buf[1]);
        /**/
        rxpackage.yaw_angle.type_2char[0] = r_buf[2];
        rxpackage.yaw_angle.type_2char[1] = r_buf[3];
        rxpackage.yaw_angle.type_2char[2] = r_buf[4];
        rxpackage.yaw_angle.type_2char[3] = r_buf[5];

        rxpackage.pitch_speed.type_2char[0] = r_buf[6];
        rxpackage.pitch_speed.type_2char[1] = r_buf[7];

        rxpackage.yaw_speed.type_2char[0] = r_buf[8];
        rxpackage.yaw_speed.type_2char[1] = r_buf[9];


        rxpackage.chassis_location.type_2char[0] = r_buf[10];
        rxpackage.chassis_location.type_2char[1] = r_buf[11];

        rxpackage.shakemode_outpostalive_mycolor_findtarget=r_buf[12];

        char findtarget_flag = (char) (r_buf[12] & 0X01);//0x01的二进制值为0000 0001，综合按位与的运算规则，可以知道取倒数第1位

        rxpackage.bfind_target=(bool) findtarget_flag;
//        cout<<armor_finder.is_cruise<<endl;

        //0:红色 1：蓝色
        char my_color = (char) (r_buf[12] & 0X02)>>1;//0x02的二进制值为0000 0010，综合按位与的运算规则，可以知道取倒数第二位
        if ((int)my_color !=1) {//如果己方不是蓝色（己方为红色）,即敌方为蓝色
            rxpackage.enemy_color=1;
        }
        else rxpackage.enemy_color=0;

        char outpost_alive = (char) (r_buf[12] & 0X04)>>2;//0x02的二进制值为0000 0100，综合按位与的运算规则，可以知道取倒数第二位
        if(int(outpost_alive)==1) rxpackage.outpost_alive=1;
        else rxpackage.outpost_alive=0;

    char shake_mode = (char) (r_buf[12] & 0X08)>>3;//0x02的二进制值为0000 0100，综合按位与的运算规则，可以知道取倒数第二位
    if(int(shake_mode)==1) rxpackage.shake_mode=1;//1:shake 0:no shake
    else rxpackage.shake_mode=0;

//        rxpackage.chassis_direction=(char) (r_buf[12] & 0X03)>>2;
        rxpackage.another_shoot_id=(short int)r_buf[13];

        rxpackage.chassis_speed.type_2char[0] = r_buf[14];
        rxpackage.chassis_speed.type_2char[1] = r_buf[15];

        rxpackage.enemy_alive_flag=r_buf[16];//00754321
        rxpackage.enemy_blood=r_buf[17];//01:1,10:5,11:3,00:4

        rxpackage.pitch_angle.type_2char[0] = r_buf[18];
        rxpackage.pitch_angle.type_2char[1] = r_buf[19];

        rxpackage.chassis_acceleration.type_2char[0] = r_buf[20];
        rxpackage.chassis_acceleration.type_2char[1] = r_buf[21];

        rxpackage.success_flag=true;

#ifdef SHOW_SERIAL_INFO
        LOGM("[READ SERIAL]:bullet_spd:%d, yaw_angle:%d, pitch_angle:%d",static_cast<short int>(rxpackage.bullet_spd),rxpackage.yaw_angle.type_short,rxpackage.pitch_angle.type_short);
        LOGM("[READ SERIAL]:yaw_spd:%d, pitch_spd:%d",rxpackage.yaw_speed.type_short,rxpackage.pitch_speed.type_short);
        LOGM("[READ SERIAL]:find_target:%d, my_color:%d, outpost_alive:%d, another id:%d",static_cast<short int>(rxpackage.chassisdirection_mycolor_findtarget),(unsigned short int)(my_color),rxpackage.outpost_alive,rxpackage.another_shoot_id);
        LOGM("[READ SERIAL]:chassis_speed:%d,,chassis_location:%d",rxpackage.chassis_speed,rxpackage.chassis_location.type_short);
#endif
//        float alpha=-PITCH_ANGLE_PLUS*(serial.rxpackage.pitch_angle.type_short-PITCH_HORIZON_ANGLE)/(10.0f);//up is zheng
//        float beta=YAW_ANGLE_PLUS*(serial.rxpackage.yaw_angle.type_short-YAW_VERTICLE_ANGLE)/(10.0f);//right is zheng
//        float gamma=0;
//        std::cout<<"alpha:"<<alpha<<" beta:"<<beta<<std::endl;
//std::cerr<<"enemy_alive"<<static_cast<short int>(rxpackage.enemy_alive_flag)<<std::endl;
//        std::cerr<<"enemy_blood"<<static_cast<short int>(rxpackage.enemy_blood)<<std::endl;

        sprintf(serial.rxpackage.read_message1,"[READ]:bullet_spd:%d, yaw_angle:%d, pitch_angle:%d,",static_cast<short int>(rxpackage.bullet_spd),rxpackage.yaw_angle.type_int,rxpackage.pitch_angle.type_short);
        sprintf(serial.rxpackage.read_message2,"       yaw_spd:%d, pitch_spd:%d, time:%lf",rxpackage.yaw_speed.type_short,rxpackage.pitch_speed.type_short,serial.t);
        sprintf(serial.rxpackage.read_message3,"       find_target:%d, my_color:%d, outpost_alive:%d, shake_mode:%d, another id:%d",static_cast<short int>(rxpackage.shakemode_outpostalive_mycolor_findtarget),(unsigned short int)(my_color),rxpackage.outpost_alive,rxpackage.shake_mode,rxpackage.another_shoot_id);
        sprintf(serial.rxpackage.read_message4,"       chss_acc(mm/s^2):%d, chss_spd(mm/s):%d, chss_loc:%d, emy_alv:%d, emy_bld:%d",rxpackage.chassis_acceleration.type_short,rxpackage.chassis_speed.type_short,rxpackage.chassis_location.type_short,static_cast<short int>(rxpackage.enemy_alive_flag),static_cast<short int>(rxpackage.enemy_blood));
#if SHOOTER_MODE==UP_SHOOTER
        serial.qChassis_locations.push(float(rxpackage.chassis_location.type_short));
//        serial.chss_spd=(float)(rxpackage.chassis_speed.type_short/1000.1f);//mm/ms
    serial.qChss_spd.push((float)(rxpackage.chassis_speed.type_short/1000.0f));//mm/ms
//    serial.chss_acc=(float)(rxpackage.chassis_acceleration.type_short/50000.0f);//mm/ms^2
//        serial.qChss_acc.push(rxpackage.chassis_acceleration.type_short/1000000.0f);//mm/ms^2
        serial.qChss_acc.push(rxpackage.chassis_acceleration.type_short/1000000.0f);//mm/ms^2
        serial.qPitches.push(PITCH_ANGLE_PLUS*(rxpackage.pitch_angle.type_short-PITCH_HORIZON_ANGLE)/10.f);
        serial.qYaws.push(YAW_ANGLE_PLUS* (rxpackage.yaw_angle.type_int-YAW_VERTICLE_ANGLE)/10.f);
//        int speed=rxpackage.pitch_speed.type_short*1024;
//        speed >>= 10;
        serial.qP_speeds.push((rxpackage.pitch_speed.type_short/100.0f));
//        speed=rxpackage.yaw_speed.type_short*1024;
//        speed >>= 10;
        serial.qY_speeds.push((rxpackage.yaw_speed.type_short/100.0f));
#elif SHOOTER_MODE==DOWN_SHOOTER
    serial.qChassis_locations.push(float(rxpackage.chassis_location.type_short));
    serial.qChss_spd.push((float)(rxpackage.chassis_speed.type_short/1000.0f));//mm/ms
    serial.qChss_acc.push(rxpackage.chassis_acceleration.type_short/1000000.0f);//mm/ms^2
    serial.qPitches.push(-PITCH_ANGLE_PLUS*(rxpackage.pitch_angle.type_short-PITCH_HORIZON_ANGLE)/10.f);
    serial.qYaws.push(-YAW_ANGLE_PLUS* (((rxpackage.yaw_angle.type_int-YAW_VERTICLE_ANGLE)%3600))/10.f);
//        int speed=rxpackage.pitch_speed.type_short*1024;
//        speed >>= 10;
    serial.qP_speeds.push(-1.0f*(rxpackage.pitch_speed.type_short/100.0f));
//        speed=rxpackage.yaw_speed.type_short*1024;
//        speed >>= 10;
    serial.qY_speeds.push(1.0f*(rxpackage.yaw_speed.type_short/100.0f));
#endif
//        std::cout<<"yaw"<<serial.rxpackage.yaw_angle.type_short<<"length:"<<serial.qYaws.length()<<"mean:"<<serial.qYaws.mean()<<std::endl;
//    std::cout<<"pspeed:"<<rxpackage.pitch_speed.type_short<<"yspeed:"<<rxpackage.yaw_speed.type_short<<std::endl;
    }
    /*这一段解决的问题：当电控数据传输到一半的时候，NUC端口已经开始接收了，故NUC只能接收到前一半的数据，剩下的位置都是0，当前数据接收错误；
     * 并且剩下的另一半数据虽然因为端口阻塞没有被接收，但是依然会停留在缓冲区，下一次接收定长数据的时候会首先读入这剩下的后半段数据，然后剩下的位置
     * 再由电控发送的数据的前半段组成，读取的数据再次错误；未读入的后半段数据又一次停留在了缓冲区，周而复始，一直出错;
     * 同时，还能清空掉缓冲区的数据，保证下一次读取到的数据是这一次读取之后电控才发送的；所以每次一定要清空一下读取缓存区。*/
    tcflush(serial.fd, TCIFLUSH);
    return true;
}

std::string get_uart_dev_name() {
    char name[20] = {0};
    //FILE *ls = popen("ls /dev/ttyUSB* --color=never", "r");//第一个为shell命令；第二个参数只能为读或写，type是"r"则文件指针连接
    // 到command的标准输出；如果type是"w"则文件指针连接到command的标准输入。
    FILE *ls = popen("ls /dev/ttyUSB*", "r");
    fscanf(ls, "%s", name);
    return name;
}

Serial::Serial(int nSpeed, char nEvent, int nBits, int nStop) :
        nSpeed(nSpeed), nEvent(nEvent), nBits(nBits), nStop(nStop) {
    qYaws.init(0);
    qPitches.init(0);
    qP_speeds.init(0);
    qY_speeds.init(0);
    qChassis_locations.init(0);
    qChss_spd.init(0);
    qChss_acc.init(0);
    if (rm_setter.wait_uart) {//uart：异步

        LOGM("Wait for serial be ready!");
        while (!InitPort(nSpeed, nEvent, nBits, nStop));
        LOGM("Port set successfully!");
    } else {
        if (InitPort(nSpeed, nEvent, nBits, nStop)) {
            LOGM("Port set successfully!");
        } else {
            LOGE("Port set fail!");
        }
    }
}

Serial::~Serial() {
    close(fd);
    fd = -1;
}

bool Serial::InitPort(int nSpeed, char nEvent, int nBits, int nStop) {
    std::string name = get_uart_dev_name();
    if (name == "") {
        return false;
    }
    if ((fd = open(name.data(), O_RDWR)) < 0                   ) {//读写方式打开文件，文件名在name.data()里面存着打开的文件名
        return false;
    }
    if (set_opt(fd, nSpeed, nEvent, nBits, nStop) < 0) {//
        return false;
    }
    fcntl(fd,F_SETFL,O_NDELAY);
    return true;
}


bool Serial::WriteData(const unsigned char *pData, unsigned int length) {
    int cnt = 0, curr = 0;
    if (fd <= 0){
        if(rm_setter.wait_uart){
            InitPort(nSpeed, nEvent, nBits, nStop);
        }
        return false;
    }
    while ((curr = write(fd, pData + cnt, length - cnt)) > 0 && (cnt += curr) < length);//write会返回发送数据的字节数
    if (curr < 0) {
        LOGW("Write serial offline!");
        close(fd);
        if (rm_setter.wait_uart) {
            InitPort(nSpeed, nEvent, nBits, nStop);
        }
        return false;
    }
    return true;
}

bool Serial::ReadData(unsigned char *buffer, unsigned int length) {
    systime t1,t2;
    getsystime(t1);
    int cnt = 0, curr = 0;
    while ((curr = read(fd, buffer + cnt, length - cnt)) >= 0 && (cnt += curr) < length&&((getsystime(t2)-t1)<20));//read函数会返回接受数字字节数；失败时返回-1
    //当read没读全数据直接返回了0,这里read的返回情况是由一开始serial设定里VMIN和VTIME决定的.并且超过20ms没有成功读入，则放弃读入
//    while(((curr =read(fd, buffer + cnt, length - cnt))==0)&&((getsystime(t2)-t1)<20));
    if(t2-t1>=20) return false;
    if (curr < 0) {
        close(fd);
        if (rm_setter.wait_uart) {
            LOGW("Read serial offline! curr:%d",curr);
            InitPort(nSpeed, nEvent, nBits, nStop);
        }
        return false;
    }
    return true;
}

int Serial::set_opt(int fd, int nSpeed, char nEvent, int nBits, int nStop) {
    termios newtio{}, oldtio{};
    if (tcgetattr(fd, &oldtio) != 0) {//tcgetattr取得终端属性
        perror("SetupSerial 1");
        return -1;
    }
    bzero(&newtio, sizeof(newtio));
    newtio.c_cflag |= CLOCAL | CREAD;//CLOCAL保证程序不会变成端口所有者；CREAD保证串口会读取数据
    newtio.c_cflag &= ~CSIZE;//与形如“c_cflag |= CS8”搭配设置字符大小

    switch (nBits) {
        case 7:
            newtio.c_cflag |= CS7;
            break;
        case 8:
            newtio.c_cflag |= CS8;
            break;
        default:
            break;
    }

    switch (nEvent) {
        case 'O':  //奇校验
            newtio.c_cflag |= PARENB;//PARENB设置则使用奇偶校验
            newtio.c_cflag |= PARODD;//PARODD设置则为奇校验；否则为偶校验
            newtio.c_iflag |= (INPCK | ISTRIP);//INPCK：允许输入奇偶校验；ISTRIP:去掉字符的第八个比特
            break;
        case 'E':  //偶校验
            newtio.c_iflag |= (INPCK | ISTRIP);
            newtio.c_cflag |= PARENB;
            newtio.c_cflag &= ~PARODD;
            break;
        case 'N':  //无校验
            newtio.c_cflag &= ~PARENB;
            break;
        default:
            break;
    }

    switch (nSpeed) {
        case 2400:
            cfsetispeed(&newtio, B2400);//设置波特率
            cfsetospeed(&newtio, B2400);
            break;
        case 4800:
            cfsetispeed(&newtio, B4800);
            cfsetospeed(&newtio, B4800);
            break;
        case 9600:
            cfsetispeed(&newtio, B9600);
            cfsetospeed(&newtio, B9600);
            break;
        case 115200:
            cfsetispeed(&newtio, B115200);
            cfsetospeed(&newtio, B115200);
            break;
        default:
            cfsetispeed(&newtio, B9600);
            cfsetospeed(&newtio, B9600);
            break;
    }

    if (nStop == 1) {
        newtio.c_cflag &= ~CSTOPB;
    } else if (nStop == 2) {
        newtio.c_cflag |= CSTOPB;//CSTOPB设置两个停止位
    }
    //如果发生数据溢出，接收数据，但是不再读取 刷新收到的数据但是不读
    //设置等待时间和最小接收字符
    /* VMIN设置成0，VTIME设置成一个小于电控发射两组数据的间隔、但大于两位数据发送时间的时间。这样如果两位数据之间的时间间隔大于VMIN（说明当前没接收到完整的一组数据）
    * 时read就会返回0，需要注意的是 VMIN设置成0时，fcntl必须设定了FNDELAY参数，不然read会一直阻塞*/
    newtio.c_cc[VTIME] = 0;/* 读取一个字符等待1*(1/10)*100ms ,哨兵电控每10ms发送一次*/
    newtio.c_cc[VMIN] = 0;//最少要接收的字符数目，如果VMIN=0，则VTIME设置为对每个字符的最大等待时间；否则为对第一个字符的最大等待时间

    if ((tcsetattr(fd, TCSANOW, &newtio)) != 0) {//TCSANOW标志所有改变立即生效；tcsetattr设置终端属性
        perror("com set error");
        return -1;
    }
    printf("set done!\n");

    return 0;
}
void showSerial(const cv::Mat& src,Txpackage txpackage,Rxpackage rxpackage){
    putText(show_pic,txpackage.send_message1,cv::Point(10, 100), cv::FONT_HERSHEY_TRIPLEX, 0.8, COLOR_WHITE);
    putText(show_pic,txpackage.send_message2,cv::Point(10, 130), cv::FONT_HERSHEY_TRIPLEX, 0.8, COLOR_WHITE);
    putText(show_pic,rxpackage.read_message1,cv::Point(10, 170), cv::FONT_HERSHEY_TRIPLEX, 0.8, cv::Scalar(255, 100, 0));
    putText(show_pic,rxpackage.read_message2,cv::Point(10, 200), cv::FONT_HERSHEY_TRIPLEX, 0.8, cv::Scalar(255, 100, 0));
    putText(show_pic,rxpackage.read_message3,cv::Point(10, 230), cv::FONT_HERSHEY_TRIPLEX, 0.8, cv::Scalar(255, 100, 0));
    putText(show_pic,rxpackage.read_message4,cv::Point(10, 260), cv::FONT_HERSHEY_TRIPLEX, 0.8, cv::Scalar(255, 100, 0));
}


